#include "rp_motor/trunk.h"

// constructor
TrunkNode::TrunkNode()  : nh_("~") {
	trunk_fstate = trunk_cstate = TRUNK_DOWN;
	timestamp_ = ros::Time::now();
	init();
	extInit();

	motor_sub_ = nh_.subscribe(Topic::MOTOR_TRUNK, 1, &TrunkNode::trunkCallback, this);
	status_pub_ = nh_.advertise<comm::trunk>(Topic::TRUNK_STATUS, 10);	// publish status
}

// destructor
TrunkNode::~TrunkNode() {
	nh_.shutdown();
}

/**
 * Init trunk node.
 */
void TrunkNode::init()
{
	// initialize motor
	ROS_INFO("Initialize trunk motor.");

	// read parameters (like the dpr2 robot)
	std::string motor_port_name, motor_config_name;

	ROS_ASSERT(nh_.getParam("motor_port", motor_port_name));
	//ROS_ASSERT(nh_.getParam("motor_config", motor_config_name));

	// load motor configuration
	//CXMLConfiguration motor_config_xml;
	//ROS_ASSERT(motor_config_xml.loadFile(motor_config_name));

	CDxlConfig *config = new CDxlConfig();
	//config.readConfig(motor_config_xml.root().section("motor"));
	motor_ = new C3mxlROS(motor_port_name.c_str());
	motor_->setConfig(config->setID(TRUNK_PORT));

	// Initialize motor
	ros::Rate init_rate(1);
	while (ros::ok() && motor_->init() != DXL_SUCCESS)
	{
		ROS_WARN_ONCE("Couldn't initialize trunk motor, will continue trying every second");
		init_rate.sleep();
	}

	delete config;
}

/**
 * Moves the motor to the start position.
 */
void TrunkNode::extInit()
{
	motor_->set3MxlMode(EXTERNAL_INIT);
	motor_->setAcceleration(10);
	motor_->setSpeed(-0.2);
	motor_->setTorque(-0.5);

	motor_->getStatus();
	while(motor_->presentStatus() == M3XL_STATUS_INITIALIZE_BUSY) {
		motor_->getStatus();	// wait
	}
	if(motor_->presentStatus() != M3XL_STATUS_INIT_DONE)
	{
		ROS_ERROR("Error trunk: %s", C3mxl::translateErrorCode(motor_->presentStatus()));
	}

	motor_->set3MxlMode(POSITION_MODE);
	motor_->setPos(TRUNK_POS_DOWN);
}

/**
 * Handle incoming messages for the trunk.
 * @param	msg	expected state for the trunk.
 */
void TrunkNode::trunkCallback(const comm::trunk::ConstPtr &msg)
{
	trunk_fstate = msg->state;
}
/**
 * Drive motor.
 */
void TrunkNode::spin()
{
	ros::Rate loop_rate(20);
	while(ros::ok())
	{
		if(trunk_fstate != trunk_cstate)
		{
			switch(trunk_fstate)
			{
			case TRUNK_NEUTRAL:
				motor_->setPos(TRUNK_POS_NEUTRAL, TRUNK_SPEED);
				break;
			case TRUNK_UP:
				motor_->setPos(TRUNK_POS_UP, TRUNK_SPEED);
				timestamp_ = ros::Time::now() + ros::Duration(1);	// 1 sec
				break;
			case TRUNK_DOWN:
				motor_->setPos(TRUNK_POS_DOWN, TRUNK_SPEED);
				timestamp_ = ros::Time::now() + ros::Duration(1);	// 1 sec
				break;
			default:
				ROS_ERROR("invalid message send to TrunkNode::spin.");
				break;
			}
			trunk_cstate = trunk_fstate;
		}

		// Publish status
		comm::trunk msg;
		if(ros::Time::now() > timestamp_)
			msg.state = trunk_cstate;
		else
			msg.state = TRUNK_NEUTRAL;
		status_pub_.publish(msg);

		ros::spinOnce();
		loop_rate.sleep();
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "trunk_motor");
	TrunkNode trunk;
	trunk.spin();
	return 0;
}
